/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <string>
#include <vector>
#include <memory>

#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "absl/memory/memory.h"
#include "slam/common/configuration_file_resolver.h"
#include "slam/common/lua_parameter_dictionary.h"
#include "slam/common/port.h"
#include "slam_ros/node_constants.h"
#include "slam_ros/ros_log_sink.h"
#include "slam_ros/trajectory_options.h"
#include "slam_ros_msgs/srv/finish_trajectory.hpp"
#include "slam_ros_msgs/srv/get_trajectory_states.hpp"
#include "slam_ros_msgs/srv/start_trajectory.hpp"
#include "slam_ros/utilities.h"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "gflags/gflags.h"
#include "rclcpp/rclcpp.hpp"
#include "slam_ros_msgs/srv/query_slam_status.hpp"
#include "agv_srvs/srv/relocate.hpp"
#include "slam/common/thread_pool.h"
#include "agv_msgs/msg/pose_with_confidence.hpp"
#include "slam_ros_msgs/msg/robot_status.hpp"

DEFINE_string(configuration_directory, "",
              "First directory in which configuration files are searched, "
              "second is always the Slam installation to allow "
              "including files from there.");
DEFINE_string(configuration_basename, "",
              "Basename, i.e. not containing any directory prefix, of the "
              "configuration file.");

DEFINE_string(initial_pose, "", "Starting pose of a new trajectory");

namespace slam_ros {
namespace {

std::shared_ptr<rclcpp::Node> nh;
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initialPoseSub;
rclcpp::Subscription<agv_msgs::msg::PoseWithConfidence>::SharedPtr curPoseSub;
rclcpp::Subscription<slam_ros_msgs::msg::RobotStatus>::SharedPtr slam_robot_statuSub;
rclcpp::TimerBase::SharedPtr startNewLocationTask_timer_;

geometry_msgs::msg::PoseWithCovarianceStamped cur_global_pose;
slam_ros_msgs::msg::RobotStatus RobotStatus_;

int getCurtActiveTrajectoryIdClient() {
  auto client = nh->create_client<slam_ros_msgs::srv::GetTrajectoryStates>(
      kGetTrajectoryStatesServiceName);

  auto request = std::make_shared<slam_ros_msgs::srv::GetTrajectoryStates::Request>();

  if (!client->wait_for_service(std::chrono::seconds(1))) {
    RCLCPP_ERROR(nh->get_logger(), "Service %s not available", kGetTrajectoryStatesServiceName);
    return -3;
  }

  auto result = client->async_send_request(request);
  if (rclcpp::spin_until_future_complete(nh, result) != 
      rclcpp::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(nh->get_logger(), "Service call failed");
    return -3;
  }

  auto response = result.get();
  int curt_active_traj = 0;
  int trajectory_states_size = response->trajectory_states.trajectory_id.size();

  if (trajectory_states_size < 2) {
    RCLCPP_ERROR(nh->get_logger(), "Trajectory_id[] size less than 2");
    return -1;
  }

  for (int i = 1; i < trajectory_states_size; i++) {
    int trajectory_id = response->trajectory_states.trajectory_id[i];
    int trajectory_state = response->trajectory_states.trajectory_state[i];
    if (trajectory_state == 0 && trajectory_id > curt_active_traj) {
      curt_active_traj = trajectory_id;
    }
  }

  if (curt_active_traj > 0) {
    RCLCPP_INFO(nh->get_logger(), "Find CurtActiveTrajectoryId: %d", curt_active_traj);
    return curt_active_traj;
  } else {
    RCLCPP_ERROR(nh->get_logger(), "No ActiveTrajectory in Trajectory list");
    return -2;
  }
}

bool finishTrajectoryClient(int trajectory_id) {
  RCLCPP_INFO(nh->get_logger(), "FinishTrajectoryClient: %d", trajectory_id);
  auto client = nh->create_client<slam_ros_msgs::srv::FinishTrajectory>(
      kFinishTrajectoryServiceName);

  auto request = std::make_shared<slam_ros_msgs::srv::FinishTrajectory::Request>();
  request->trajectory_id = trajectory_id;

  if (!client->wait_for_service(std::chrono::seconds(1))) {
    RCLCPP_ERROR(nh->get_logger(), "Service %s not available", kFinishTrajectoryServiceName);
    return false;
  }

  auto result = client->async_send_request(request);
  if (rclcpp::spin_until_future_complete(nh, result) != 
      rclcpp::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(nh->get_logger(), "Service call failed");
    return false;
  }

  auto response = result.get();
  int resp_code = response->status.code;
  std::string resp_msg = response->status.message;

  RCLCPP_INFO(nh->get_logger(), "%d: %s", resp_code, resp_msg.c_str());
  return (resp_code == 0);
}

bool startTrajectoryClient(
    const geometry_msgs::msg::PoseWithCovarianceStamped pose_msg) {
  if (pose_msg.header.frame_id != "map") {
    RCLCPP_ERROR(nh->get_logger(), "Given estimate pose not in \"map\" frame");
    return false;
  }

  RCLCPP_WARN(nh->get_logger(), "Setting Estimate Pose!");

  auto client = nh->create_client<slam_ros_msgs::srv::StartTrajectory>(
      kStartTrajectoryServiceName);

  auto request = std::make_shared<slam_ros_msgs::srv::StartTrajectory::Request>();
  std::string config_basename = FLAGS_configuration_basename;

  {
    auto slam_status_client = nh->create_client<slam_ros_msgs::srv::QuerySlamStatus>(kQuerySlamStatus);
    auto slam_status_request = std::make_shared<slam_ros_msgs::srv::QuerySlamStatus::Request>();
    slam_status_request->queryslam = true;

    if (slam_status_client->wait_for_service(std::chrono::seconds(1))) {
      auto slam_status_result = slam_status_client->async_send_request(slam_status_request);
      if (rclcpp::spin_until_future_complete(nh, slam_status_result) == 
          rclcpp::FutureReturnCode::SUCCESS) {
        auto response = slam_status_result.get();
        const std::string map_file_name = response->mapfile;
        const int pos = map_file_name.find_last_of('/');
        const std::string config_file = (pos == std::string::npos)
            ? map_file_name + ".lua"
            : map_file_name.substr(pos + 1) + ".lua";
        if (fileExists(FLAGS_configuration_directory + "/" + config_file)) {
          config_basename = config_file;
          RCLCPP_WARN(nh->get_logger(), "Localization: Using config %s", config_file.c_str());
        }
      }
    }
  }
  
  request->configuration_directory = FLAGS_configuration_directory;
  request->configuration_basename = config_basename;
  request->use_initial_pose = true;
  request->initial_pose = pose_msg.pose.pose;
  request->relative_to_trajectory_id = 0;

  if (!client->wait_for_service(std::chrono::seconds(1))) {
    RCLCPP_ERROR(nh->get_logger(), "Service %s not available", kStartTrajectoryServiceName);
    return false;
  }

  auto result = client->async_send_request(request);
  if (rclcpp::spin_until_future_complete(nh, result) != 
      rclcpp::FutureReturnCode::SUCCESS) {
    RCLCPP_ERROR(nh->get_logger(), "Service call failed");
    return false;
  }

  auto response = result.get();
  RCLCPP_WARN(nh->get_logger(), "InitialPose: startTrajectoryClient %d", response->trajectory_id);
  return true;
}

void HandleInitialPose(
    const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr pose_msg) {
  RCLCPP_WARN(nh->get_logger(), "Receive Pose Estimation Request!");

  if (pose_msg->header.frame_id != "map") {
    RCLCPP_ERROR(nh->get_logger(), "Given estimate pose not in \"map\" frame");
    return;
  }

  int id_to_finish = getCurtActiveTrajectoryIdClient();
  if (id_to_finish < 0) {
    std::string err_msg = "Could not get current active trajectory";
    RCLCPP_ERROR(nh->get_logger(), "Slam InitialPose call failed: %s", err_msg.c_str());
    return;
  }

  bool finish_status = finishTrajectoryClient(id_to_finish);
  if (!finish_status) {
    std::string err_msg = "Could not finish current active trajectory";
    RCLCPP_ERROR(nh->get_logger(), "Slam InitialPose call failed: %s", err_msg.c_str());
    return;
  }

  bool start_status = startTrajectoryClient(*pose_msg);
  if (!start_status) {
    std::string err_msg = "Could not start new active trajectory";
    RCLCPP_ERROR(nh->get_logger(), "Slam InitialPose call failed: %s", err_msg.c_str());
    return;
  }
}

void HandleInitialPoseSrvCallback(
    const std::shared_ptr<agv_srvs::srv::Relocate::Request> request,
    std::shared_ptr<agv_srvs::srv::Relocate::Response> response) {
  RCLCPP_INFO(nh->get_logger(), "agvRelocate : reset initPose by srv");
  if (request->mode.data == 1) {
    auto cur_msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
    cur_msg->pose.pose.position.x = request->point_yaw.x;
    cur_msg->pose.pose.position.y = request->point_yaw.y;

    tf2::Quaternion q;
    q.setRPY(0.0, 0.0, request->point_yaw.theta);
    cur_msg->pose.pose.orientation = tf2::toMsg(q);
    cur_msg->header.frame_id = "map";
    HandleInitialPose(cur_msg);

    if (true) {
      response->success = 1;
    } else {
      response->success = 0;
      response->error =
          "Positioning accuracy does not meet the requirements, please try "
          "again";
    }

    RCLCPP_INFO(nh->get_logger(), "finished setting position: %f %f %f", 
                request->point_yaw.x, request->point_yaw.y, request->point_yaw.theta);
  }
}

void HandeCurPose(
    const agv_msgs::msg::PoseWithConfidence::SharedPtr pose_msg) {
  cur_global_pose.pose.pose.position.x = pose_msg->pose.pose.position.x;
  cur_global_pose.pose.pose.position.y = pose_msg->pose.pose.position.y;
  cur_global_pose.pose.pose.orientation = pose_msg->pose.pose.orientation;
  cur_global_pose.header.frame_id = "map";
}

void HandeSlamStatus(
    const slam_ros_msgs::msg::RobotStatus::SharedPtr statue) {
  RobotStatus_.slam_type.data = statue->slam_type.data;
  RobotStatus_.localization_map = statue->localization_map;
  RobotStatus_.laser_to_base = statue->laser_to_base;
  RobotStatus_.localization_state.data = statue->localization_state.data;
  RobotStatus_.localization_pose = statue->localization_pose;
}

void StartLocationTaskByOtherNav() {
  // Timer callback implementation
}

void Run() {
  initialPoseSub = nh->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
      "/initialpose", 1, std::bind(&slam_ros::HandleInitialPose, std::placeholders::_1));

  curPoseSub = nh->create_subscription<agv_msgs::msg::PoseWithConfidence>(
      "/pose", 1, std::bind(&slam_ros::HandeCurPose, std::placeholders::_1));

  slam_robot_statuSub = nh->create_subscription<slam_ros_msgs::msg::RobotStatus>(
      "/robot_status", 1, std::bind(&slam_ros::HandeSlamStatus, std::placeholders::_1));

  startNewLocationTask_timer_ = nh->create_wall_timer(
      std::chrono::seconds(1), std::bind(&slam_ros::StartLocationTaskByOtherNav));
}

}  // namespace
}  // namespace slam_ros

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);
  google::SetUsageMessage(
      "\n\n"
      "Convenience tool around the start_trajectory service. This takes a Lua "
      "file that is accepted by the node as well and starts a new trajectory "
      "using its settings.\n");
  google::ParseCommandLineFlags(&argc, &argv, true);

  CHECK(!FLAGS_configuration_directory.empty())
      << "-configuration_directory is missing.";
  CHECK(!FLAGS_configuration_basename.empty())
      << "-configuration_basename is missing.";

  rclcpp::init(argc, argv);
  slam_ros::nh = std::make_shared<rclcpp::Node>("slam_start_trajectory_initial_pose");

  auto slam_setInitPose_task_server = slam_ros::nh->create_service<agv_srvs::srv::Relocate>(
      ::slam_ros::ksetInitPoseTask, 
      std::bind(&slam_ros::HandleInitialPoseSrvCallback, 
                std::placeholders::_1, std::placeholders::_2));

  slam_ros::ScopedRosLogSink ros_log_sink;
  slam_ros::Run();

  int forceRelocate_ = 0;
  int last_forceRelocate_ = 0;
  int cur_nav_mode = -1;
  
  rclcpp::Rate loop_rate(1);
  while (rclcpp::ok()) {
    rclcpp::spin_some(slam_ros::nh);
    
    slam_ros::nh->get_parameter("/forceRelocate", forceRelocate_);
    slam_ros::nh->get_parameter("/AGVParam/agv_navigation_type", cur_nav_mode);
    
    if(forceRelocate_ != 0 && forceRelocate_ != last_forceRelocate_){
      auto pose_msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(slam_ros::cur_global_pose);
      slam_ros::HandleInitialPose(pose_msg);
      RCLCPP_INFO(slam_ros::nh->get_logger(), 
                 "cur nav mode is %d, lastForceRelocateMark is %d, curForceRelocateMark is %d",
                 cur_nav_mode, last_forceRelocate_, forceRelocate_);

      last_forceRelocate_ = forceRelocate_;
    }
    loop_rate.sleep();
  }
  
  rclcpp::shutdown();
  return 0;
}